#include <iostream>
#include <ilcplex/ilocplex.h>
#include <fstream>
#include <visualization_msgs/Marker.h>
#include <cmath>
#include "HGJPathSmooth/pathSmoother.h"
#include <ros/ros.h>
#include <ros/package.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Path.h>
#include <visualization_msgs/MarkerArray.h>
#include <mavros_msgs/State.h>
#include <geometry_msgs/TwistStamped.h>
#include <string>
#include "Eigen/Eigen"
#include "math.h"
#include <PosVelAcc.h>
#include <boost/filesystem.hpp>

using namespace std;
using namespace Eigen;

/**************************** Global Variable *******************************/
mavros_msgs::State uav_cur_state;
geometry_msgs::PoseStamped uav_cur_pose;
geometry_msgs::PoseStamped takeoff_pose; 
geometry_msgs::PoseStamped offb_hover_pose; // pose to hover in offboard mode

std::vector<geometry_msgs::PoseStamped> uav_pose_preview_vec;
std::vector<geometry_msgs::PoseStamped> uav_pose_curr_vec;
geometry_msgs::PoseStamped pose_preview; // 发布的目标姿态
geometry_msgs::PoseStamped pose_curr;    // 发布的当前姿态
float hover_height = 1.0;
float tf_scale = 1.0;
float tf_theta = 0;
float tf_x = 0.0;
float tf_y = 0.0;
float tf_z = 0.0;

int ctrl_rate = 25;
bool repeat_path = false;

ros::Publisher traj_curr_pose_pub;
ros::Publisher traj_curr_path_pub;

/**************************** Function Declaration and Definition *******************************/
// Declaration Function
void path_visual(std::vector<geometry_msgs::PoseStamped> &pose_msg_vec, geometry_msgs::PoseStamped &pose_msg, ros::Publisher &pub);

// Tool Function
template <typename T>
void readParam(ros::NodeHandle &nh, std::string param_name, T &loaded_param)
{
    // template to read param from roslaunch
    const std::string &node_name = ros::this_node::getName();
    param_name = node_name + "/" + param_name;
    if (!nh.getParam(param_name, loaded_param))
    {
        ROS_WARN_STREAM("Fail to load " << param_name << ", use default value:" << loaded_param);
    }
    else
    {
        ROS_INFO_STREAM("Load successfully " << param_name << ": " << loaded_param);
    }
}

void pose2pva(geometry_msgs::PoseStamped pose, offboard::PosVelAcc &pva)
{
    pva.px = pose.pose.position.x;
    pva.py = pose.pose.position.y;
    pva.pz = pose.pose.position.z;
    pva.vx = 0.0;
    pva.vy = 0.0;
    pva.vz = 0.0;
    pva.ax = 0.0;
    pva.ay = 0.0;
    pva.az = 0.0;
    pva.yaw = 0.0;
}

void loadRosParams(ros::NodeHandle &nh)
{
    readParam<int>(nh, "ctrl_rate", ctrl_rate);
    readParam<bool>(nh, "repeat_path", repeat_path);
    readParam<float>(nh, "hover_height", hover_height);
    readParam<float>(nh, "tf_scale", tf_scale);
    readParam<float>(nh, "tf_theta", tf_theta);
    readParam<float>(nh, "tf_x", tf_x);
    readParam<float>(nh, "tf_y", tf_y);
    readParam<float>(nh, "tf_z", tf_z);
}

// callback function
void uav_state_cb(const mavros_msgs::State::ConstPtr &msg)
{
    uav_cur_state = *msg;
}

void uav_pose_cb(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
    uav_cur_pose = *msg;
    // 实时查看当前飞机位姿与轨迹
    pose_curr = uav_cur_pose;
    pose_curr.header.frame_id = "global";
    pose_curr.header.stamp = ros::Time::now();
    traj_curr_pose_pub.publish(pose_curr);                         // pub pose
    path_visual(uav_pose_curr_vec, pose_curr, traj_curr_path_pub); // pub path
}

void path_visual(std::vector<geometry_msgs::PoseStamped> &pose_msg_vec, geometry_msgs::PoseStamped &pose_msg, ros::Publisher &pub)
{
    // 填充消息
    pose_msg_vec.emplace_back(pose_msg);

    nav_msgs::Path path_msg;
    path_msg.header = pose_msg.header;
    for (auto &i : pose_msg_vec)
    {
        path_msg.poses.push_back(i);
    }
    // 发布数据
    pub.publish(path_msg);
}
/**
 *  POINTS类型在添加的每个点处放置一个点。
 *  LINE_STRIP类型将每个点用作一组连接的线中的顶点，其中点0连接到点1，点1连接到点2，点2连接到点3等。
 *  LINE_LIST类型在每对点（即点0到1、2到3等）中创建未连接的线。
 */

int main( int argc, char** argv )
{
    ros::init(argc, argv, "points_and_lines");
    ros::NodeHandle n;
    
    // ros init
    ros::init(argc, argv, "offboard_node", ros::init_options::AnonymousName);
    ros::NodeHandle nh;
    ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("/pathpoints_visualization_marker", 10);

    // load param
    loadRosParams(nh);
    std::string pkg_path = ros::package::getPath("offboard");
    
    // ros pub and sub
    ros::Subscriber uav_state_sub = nh.subscribe<mavros_msgs::State>("/mavros/state", 10, uav_state_cb);
    ros::Subscriber uav_pose_sub = nh.subscribe<geometry_msgs::PoseStamped>("/mavros/vision_pose/pose", 2, uav_pose_cb);
    ros::Publisher offb_setpva_pub = nh.advertise<offboard::PosVelAcc>("/setpoint_pva", 10);

    ros::Publisher traj_online_preview_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/traj_preview/online/pose", 10);
    ros::Publisher traj_online_preview_path_pub = nh.advertise<nav_msgs::Path>("/traj_preview/online/path", 10);

    traj_curr_pose_pub = nh.advertise<geometry_msgs::PoseStamped>("/traj_curr/online/pose", 10);
    traj_curr_path_pub = nh.advertise<nav_msgs::Path>("/traj_curr/online/path", 10);

    // the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate ctrl_loop(ctrl_rate);

    int traj_index = 0;

    HGJ::pathPlanner planner;
    HGJ::WayPoints wayPoints;

    while (ros::ok())
    {
        
        ros::spinOnce();

        wayPoints = {{0,0,0.8},
                     {0,0.5,0.8},
                     {0,1.0,0.8},
                     {0.5,0.5,0.8},
                     {1.0,0,0.8},
                     {1,0.5,0.8},
                     {1,1,0.8},
                     {0.5,0.5,0.8},
                     {0,0,0.8},
                     {0,0,0}};

        // 生成路径曲线
        auto smoothedPath = planner.genCurv(wayPoints, 0.25, 0.08);

        // 定义 Eigen 矩阵
        Eigen::Matrix<double, Eigen::Dynamic, 3> csv_point;

        // 调整矩阵大小以匹配路径点数量
        csv_point.resize(smoothedPath.size(), 3);

        // 填充矩阵
        for (int i = 0; i < smoothedPath.size(); ++i) {
            csv_point.row(i) << smoothedPath[i].getX(), smoothedPath[i].getY(), smoothedPath[i].getZ();
        }

        // 打印矩阵
        for (int i = 0; i < csv_point.rows(); ++i) {
            for (int j = 0; j < csv_point.cols(); ++j) {
                std::cout << csv_point(i, j) << " ";
            }
            std::cout << std::endl; // 每行打印完后换行
        }

        // 创建一个足够大的矩阵        
        Eigen::MatrixXd csv_traj(csv_point.rows(), 9); 

        // 填充矩阵的前三列
        for (int i = 0; i < csv_point.rows(); ++i) {
            csv_traj(i, 0) = csv_point(i, 0); // px
            csv_traj(i, 1) = csv_point(i, 1); // py
            csv_traj(i, 2) = csv_point(i, 2); // pz
        }

        // 设置第一行和第二行的速度和加速度为0
        csv_traj.row(0).tail(6).setZero();
        csv_traj.row(1).tail(3).setZero();

        // 计算速度和加速度
        for (int i = 0; i < csv_traj.rows() - 1; ++i) {
            for (int j = 0; j < 3; ++j) {
                // 简单的差分来计算速度
                csv_traj(i, j + 3) = csv_traj(i + 1, j) - csv_traj(i, j);
                // 对于加速度，需要计算连续的速度差
                if (i < csv_traj.rows() - 2) {
                    csv_traj(i, j + 6) = csv_traj(i + 1, j + 3) - csv_traj(i, j + 3);
                }
            }
        } 

        // wait for FCU connection
        while (ros::ok() && !uav_cur_state.connected)
        {
            ros::spinOnce();
            ROS_INFO_THROTTLE(1, "wait for FCU connection");
            ctrl_loop.sleep();
        }

        /**
         * Pre Step :
         * Send a few setpoints before starting
         * Before entering Offboard mode,
         * you must have already started streaming setpoints.
         * Otherwise the mode switch will be rejected.
         */
        offboard::PosVelAcc pva_before_offb;
        for (int i = 10; ros::ok() && i > 0; --i)
        {
            ros::spinOnce();
            pose2pva(uav_cur_pose, pva_before_offb);
            offb_setpva_pub.publish(pva_before_offb);
            ctrl_loop.sleep();
        }

        bool takeoff = false;  // Record whether the uav has taken off
        bool just_takeoff = true; // check whether plane change into offboard mode from other mode

        geometry_msgs::PoseStamped pose_offb_hover; // pose to hover in offboard mode
        offboard::PosVelAcc pva_offb_hover;
        pose2pva(uav_cur_pose, pva_offb_hover);


        ros::spinOnce();
            if (uav_cur_state.armed)
            {
                if (uav_cur_state.mode == "OFFBOARD")
                {
                    ROS_INFO_STREAM_THROTTLE(1, "\033[33m OFFBOARD Mode \033[0m");

                    // Take off to some height
                    if (!takeoff)
                    {
                        offboard::PosVelAcc pva_climb;
                        geometry_msgs::PoseStamped climb_pose; // 预计爬升的高度
                        takeoff_pose.pose.position.x = uav_cur_pose.pose.position.x;
                        takeoff_pose.pose.position.y = uav_cur_pose.pose.position.y;
                        takeoff_pose.pose.position.z = hover_height;
                        climb_pose.pose.position.z = uav_cur_pose.pose.position.z;

                        double err = 0.1; // TODO: 高度阈值
                        while (fabs(takeoff_pose.pose.position.z - uav_cur_pose.pose.position.z) > err)
                        {
                            ros::spinOnce();
                            climb_pose.pose.position.x = takeoff_pose.pose.position.x;
                            climb_pose.pose.position.y = takeoff_pose.pose.position.y;
                            climb_pose.pose.position.z = climb_pose.pose.position.z + 0.2 / double(ctrl_rate); // TODO:上升速度 0.2m/s
                            climb_pose.pose.position.z = std::min(climb_pose.pose.position.z, takeoff_pose.pose.position.z);
                            pose2pva(climb_pose, pva_climb);
                            offb_setpva_pub.publish(pva_climb);
                            ROS_INFO("[Takeoff]: Target xyz: %.3f, %.3f, %.3f | Curr xyz: %.3f, %.3f, %.3f",
                                    climb_pose.pose.position.x, climb_pose.pose.position.y, climb_pose.pose.position.z,
                                    uav_cur_pose.pose.position.x, uav_cur_pose.pose.position.y, uav_cur_pose.pose.position.z);
                            ctrl_loop.sleep();
                        }
                        ROS_INFO_STREAM("\033[33m Take off done \033[0m");
                        takeoff = true;
                        offb_hover_pose = takeoff_pose;
                    }

                    // TODO: Switch into offboard mode just now, stay still for a while, about several seconds
                    if (just_takeoff)
                    {
                        for (int i = 150; ros::ok() && i > 0; --i)
                        {
                            ros::spinOnce();
                            pose2pva(takeoff_pose, pva_before_offb);
                            offb_setpva_pub.publish(pva_before_offb);
                            ctrl_loop.sleep();
                            ROS_INFO("Before trajectory begin, wait for several seconds ...");
                        }
                        just_takeoff = false;
                        offb_hover_pose = takeoff_pose;
                    }

                    ROS_INFO_ONCE("In Offboard mode, start trajectory moving !");
                    if (traj_index + 1 < csv_traj.size())
                    {
                        offboard::PosVelAcc pva_traj;
                        pva_traj.header.frame_id = "global";
                        pva_traj.header.stamp = ros::Time::now();
                        pva_traj.px = csv_traj(traj_index, 0) + takeoff_pose.pose.position.x;
                        pva_traj.py = csv_traj(traj_index, 1) + takeoff_pose.pose.position.y;
                        pva_traj.pz = csv_traj(traj_index, 2) + hover_height;
                        pva_traj.vx = csv_traj(traj_index, 3);
                        pva_traj.vy = csv_traj(traj_index, 4);
                        pva_traj.vz = csv_traj(traj_index, 5);
                        pva_traj.ax = csv_traj(traj_index, 6);
                        pva_traj.ay = csv_traj(traj_index, 7);
                        pva_traj.az = csv_traj(traj_index, 8);

                        offb_setpva_pub.publish(pva_traj);

                        geometry_msgs::PoseStamped traj_pose;
                        traj_pose.pose.position.x = csv_traj(traj_index, 0) + takeoff_pose.pose.position.x;
                        traj_pose.pose.position.y = csv_traj(traj_index, 1) + takeoff_pose.pose.position.y;
                        traj_pose.pose.position.z = csv_traj(traj_index, 2) + hover_height;
                        traj_pose.header.frame_id = "global";
                        traj_pose.header.stamp = ros::Time::now();
                        pose_preview = traj_pose;
                        pose_offb_hover = uav_cur_pose;
                        traj_index++;
                        ROS_INFO("[trajectory] xyz: %.3f, %.3f, %.3f", traj_pose.pose.position.x, traj_pose.pose.position.y, traj_pose.pose.position.z);
                    }
                    else
                    {
                        if (repeat_path)
                        {
                            ROS_WARN("=============== Repeat the trajectory ===============");
                            traj_index = 0; // 从头开始 pva模式不要重复路径
                        }
                        else
                        {
                            ROS_WARN_THROTTLE(1, "=============== Trajectory finished, Hover ... ===============");
                            pose2pva(pose_offb_hover, pva_offb_hover);
                            offb_setpva_pub.publish(pva_offb_hover);
                            pose_preview = pose_offb_hover;
                        }
                    }
                }
                else
                { // If not offb
                    ROS_INFO_STREAM_THROTTLE(1, " waiting for Offboard cmd");
                    offboard::PosVelAcc pva_curr;
                    pose2pva(uav_cur_pose, pva_curr);
                    offb_setpva_pub.publish(pva_curr);
                    pose_preview = uav_cur_pose;
                    just_takeoff = true;
                }
            }
            else
            { // If not arm
                ROS_INFO_STREAM_THROTTLE(1, " waiting for Vehicle arm");
                offboard::PosVelAcc pva_curr;
                pose2pva(uav_cur_pose, pva_curr);
                offb_setpva_pub.publish(pva_curr);
                pose_preview = uav_cur_pose;
                takeoff = false;
                just_takeoff = true;
            }

            // 实时预览当前发布的目标位姿与轨迹
            pose_preview.header.frame_id = "global";
            pose_preview.header.stamp = ros::Time::now();
            traj_online_preview_pose_pub.publish(pose_preview);                            // pub pose
            path_visual(uav_pose_preview_vec, pose_preview, traj_online_preview_path_pub); // pub path

            ctrl_loop.sleep();
        }

    return 0;
}